/**
 * @file:          MotorRippleCompensator.c
 * @brief:         
 * @details:       
 * @author:        wjh
 * @date created:  2023.08.03
 * @version:       1.0.0
 * @par copyright (c):
 *   
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.08.03,18:13:24
 */


/* Include Files **************************************************************/
#include "MotorRippleCompensator.h"
#include "common.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/

void MotorRippleCompensator_Init(struct MotorRippleCompensator *self)
{
    self->Run = MotorRippleCompensator_Run;
}

float32 MotorRippleCompensator_Run(struct MotorRippleCompensator *self, float32 theta)
{
    float32 iq_cps = 0;

    if (self->en)
    {
        for (size_t i = 0; i < NUM_OF_SIN_TERM; i++)
        {
            iq_cps += self->sin_coeffs[i].term.A*sin_bm(self->sin_coeffs[i].term.n*theta + self->sin_coeffs[i].term.phi);
        }
        iq_cps *= self->param_gain;
    }

    iq_cps = Limit(iq_cps, -self->param_limit, self->param_limit);
    
    return iq_cps;
}